#include "function.h"
#include <bitset>
std::vector<double> record_theta_tan ={};
std::vector<double> record_theta_cos ={};
std::vector<double> record_theta_sin ={};
// 两个包围盒的碰撞检测，OOB的表达形式，一个中心点，三个旋转轴(就是垂直于对应面的方向角，例如Axis就是垂直x面的方向角)，三个边长/半边长
// 检查所选轴之间是否有分离平面
// 只能在靠近的区域使用这种方法，全部使用的话计算量太大
// 而且我们应该先从父节点开始判断，如果其不在父节点中，那么他的所有子节点就不用在判断了

//抛弃不可能的碰撞 即 矩形下边缘 > -1.1
bool abortTesting(const float& znodedown,const float& zrobotup,const float& znodeup,const float& zrobotdown) {
    return (znodedown >= zrobotup || znodeup <= zrobotdown);
}
//检测是否存在分离平面/分离轴
bool isSeparatingPlane(const Vector& RPos, const Vector& Plane, const Cuboid& box1, const Cuboid& box2)
{
    return (fabs(RPos * Plane) >
            (fabs((box1.m_axis[0] * box1.m_halflength) * Plane) +
            fabs((box1.m_axis[1] * box1.m_halfwidth) * Plane) +
            fabs((box1.m_axis[2] * box1.m_halfheight) * Plane) +
            fabs((box2.m_axis[0] * box2.m_halflength) * Plane) +
            fabs((box2.m_axis[1] * box2.m_halfwidth) * Plane)+
            fabs((box2.m_axis[2] * box2.m_halfheight) * Plane)
         ));
}

//通过OBB包围盒判断碰撞 3D
bool isCollisionOBB(const Cuboid& box1, const Cuboid& box2)
{
    static Point RPos;
    RPos = box2.m_center - box1.m_center;
    return !(
        isSeparatingPlane(RPos, box1.m_axis[0], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[1], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[2], box1, box2) ||
        isSeparatingPlane(RPos, box2.m_axis[0], box1, box2) ||
        isSeparatingPlane(RPos, box2.m_axis[1], box1, box2) ||
        isSeparatingPlane(RPos, box2.m_axis[2], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[0] ^ box2.m_axis[0], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[0] ^ box2.m_axis[1], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[0] ^ box2.m_axis[2], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[1] ^ box2.m_axis[0], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[1] ^ box2.m_axis[1], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[1] ^ box2.m_axis[2], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[2] ^ box2.m_axis[0], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[2] ^ box2.m_axis[1], box1, box2) ||
        isSeparatingPlane(RPos, box1.m_axis[2] ^ box2.m_axis[2], box1, box2)
        );
}

//OBB分离轴测试========改进，更加健壮和高效
//健壮，解决两个测试单元平行时叉积为零(近似),可能导致的错误(相较判断为分离)
//高效，通过将坐标转换到其中一个测试单元来减少测试时每一个轴需要的坐标转换
bool isCollisionOBB_V2(const Cuboid &a, const Cuboid &b)
{
    float ra, rb;
    Vector R[3], absR[3];

    // Compute rotation matrix expressing b in a's coordinate frame
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            R[i][j] = a.m_axis[i]*b.m_axis[j];

    // Compute translation vector t
    Vector t = b.m_center - a.m_center;
    // Bring translation into a's coordinate frame
    t = Vector(t*a.m_axis[0], t*a.m_axis[1], t*a.m_axis[2]);

    // Compute common subexpressions. Add in an epsilon term to
    // counteract arithmetic errors when two edges are parallel and
    // their cross product is (near) null (see text for details)
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            absR[i][j] = fabs(R[i][j]) + EPSILON;

    // Test axes L = A0, L = A1, L = A2
    for (int i = 0; i < 3; i++) {
        ra = a[i];
        rb = b[0] * absR[i][0] + b[1] * absR[i][1] + b[2] * absR[i][2];
        if (fabs(t[i]) > ra + rb) return false;
    }

    // Test axes L = B0, L = B1, L = B2
    for (int i = 0; i < 3; i++) {
        ra = a[0] * absR[0][i] + a[1] * absR[1][i] + a[2] * absR[2][i];
        rb = b[i];
        if (fabs(t[0] * R[0][i] + t[1] * R[1][i] + t[2] * R[2][i]) > ra + rb) return false;
    }

    // Test axis L = A0 x B0
    ra = a[1] * absR[2][0] + a[2] * absR[1][0];
    rb = b[1] * absR[0][2] + b[2] * absR[0][1];
    if (fabs(t[2] * R[1][0] - t[1] * R[2][0]) > ra + rb) return false;

    // Test axis L = A0 x B1
    ra = a[1] * absR[2][1] + a[2] * absR[1][1];
    rb = b[0] * absR[0][2] + b[2] * absR[0][0];
    if (fabs(t[2] * R[1][1] - t[1] * R[2][1]) > ra + rb) return false;

    // Test axis L = A0 x B2
    ra = a[1] * absR[2][2] + a[2] * absR[1][2];
    rb = b[0] * absR[0][1] + b[1] * absR[0][0];
    if (fabs(t[2] * R[1][2] - t[1] * R[2][2]) > ra + rb) return false;

    // Test axis L = A1 x B0
    ra = a[0] * absR[2][0] + a[2] * absR[0][0];
    rb = b[1] * absR[1][2] + b[2] * absR[1][1];
    if (fabs(t[0] * R[2][0] - t[2] * R[0][0]) > ra + rb) return false;

    // Test axis L = A1 x B1
    ra = a[0] * absR[2][1] + a[2] * absR[0][1];
    rb = b[0] * absR[1][2] + b[2] * absR[1][0];
    if (fabs(t[0] * R[2][1] - t[2] * R[0][1]) > ra + rb) return false;

    // Test axis L = A1 x B2
    ra = a[0] * absR[2][2] + a[2] * absR[0][2];
    rb = b[0] * absR[1][1] + b[1] * absR[1][0];
    if (fabs(t[0] * R[2][2] - t[2] * R[0][2]) > ra + rb) return false;

    // Test axis L = A2 x B0
    ra = a[0] * absR[1][0] + a[1] * absR[0][0];
    rb = b[1] * absR[2][2] + b[2] * absR[2][1];
    if (fabs(t[1] * R[0][0] - t[0] * R[1][0]) > ra + rb) return false;

    // Test axis L = A2 x B1
    ra = a[0] * absR[1][1] + a[1] * absR[0][1];
    rb = b[0] * absR[2][2] + b[2] * absR[2][0];
    if (fabs(t[1] * R[0][1] - t[0] * R[1][1]) > ra + rb) return false;

    // Test axis L = A2 x B2
    ra = a[0] * absR[1][2] + a[1] * absR[0][2];
    rb = b[0] * absR[2][1] + b[1] * absR[2][0];
    if (fabs(t[1] * R[0][2] - t[0] * R[1][2]) > ra + rb) return false;

    // Since no separating axis found, the OBBs must be intersecting
    return true;
}

bool isSeparatingLine(const Cuboid& box1, const Cuboid& box2,const Vector& axis,const Vector& delta){
    return box1.m_halflength*fabs(axis*box1.m_axis[0]) + box1.m_halfwidth*fabs(axis*box1.m_axis[1])\
          +box2.m_halflength*fabs(axis*box2.m_axis[0]) + box2.m_halfwidth*fabs(axis*box2.m_axis[1])\
          <=fabs(delta*axis);
}
//通过OBB包围盒判断碰撞 2D
bool isCollisionOBB2D(const Cuboid& box1, const Cuboid& box2){
    Point delta = box1.m_center - box2.m_center;
    if(isSeparatingLine(box1,box2,box1.m_axis[0],delta)) return false;
    if(isSeparatingLine(box1,box2,box1.m_axis[1],delta)) return false;
    if(isSeparatingLine(box1,box2,box2.m_axis[0],delta)) return false;
    if(isSeparatingLine(box1,box2,box2.m_axis[1],delta)) return false;
    return true;
}


//通过二维包围球获得碰撞
bool isCollisionSphere(const Cuboid& octree, const Cuboid& robot) {
    //std::cout << "octree.m_R2 = " << octree.m_R2 << std::endl;
    //std::cout << "robot.m_R2 = " << robot.m_R2 << std::endl;
    return (sqrt(((octree.m_center.m_x-robot.m_center.m_x)*(octree.m_center.m_x - robot.m_center.m_x)
        + (octree.m_center.m_y - robot.m_center.m_y) * (octree.m_center.m_y - robot.m_center.m_y)))
        <= (octree.m_R2 + robot.m_R2));
}

//通过AABB包围盒获得碰撞
//通过AABB包围盒获得碰撞
bool isCollisionAABB(const Cuboid& octree,const Cuboid& robot){
    float maxX = std::max(robot.UR.m_x,std::max(robot.DR.m_x,std::max(robot.DL.m_x,robot.UL.m_x)));
    float minX = std::min(robot.UR.m_x,std::min(robot.DR.m_x,std::min(robot.DL.m_x,robot.UL.m_x)));
    float maxY = std::max(robot.UR.m_y,std::max(robot.DR.m_y,std::max(robot.DL.m_y,robot.UL.m_y)));
    float minY = std::min(robot.UR.m_y,std::min(robot.DR.m_y,std::min(robot.DL.m_y,robot.UL.m_y)));
    if(octree.m_center.m_x >= robot.m_center.m_x && octree.m_center.m_y >= robot.m_center.m_y){
        if(maxX >= octree.m_center.m_x-octree.m_halflength\
         &&maxY  >= octree.m_center.m_y-octree.m_halfwidth)
            return true;
    }
    else if(octree.m_center.m_x >= robot.m_center.m_x && octree.m_center.m_y <= robot.m_center.m_y){
        if(maxX >= octree.m_center.m_x-octree.m_halflength\
         &&minY  <= octree.m_center.m_y+octree.m_halfwidth)
            return true;
    }
    else if(octree.m_center.m_x <= robot.m_center.m_x && octree.m_center.m_y <= robot.m_center.m_y){
        if(minX <= octree.m_center.m_x+octree.m_halflength\
         &&minY  <= octree.m_center.m_y+octree.m_halfwidth)
            return true;
    }
    else if(octree.m_center.m_x <= robot.m_center.m_x && octree.m_center.m_y >= robot.m_center.m_y){
        if(minX <= octree.m_center.m_x+octree.m_halflength\
         &&maxY  >= octree.m_center.m_y-octree.m_halfwidth)
            return true;
    }

    return false;
}



//点和长方体的碰撞检测，有一个特点就是该长方体的高是固定的，可以简化成矩形和点的碰撞，利用凸多边形的射线方法检测
//但是对于长方形这种简单的凸多边形，利用投影法则就可以完成了
bool isCollisionProject(Point* p,const Cuboid& robot) {
    //抛弃z方向上不可能的点
    if (p->m_z > robot.m_center.m_z+robot.m_halfheight ||\
        p->m_z < robot.m_center.m_z-robot.m_halfheight)
        return false;
    //矩形中心和点的连线矢量
    Point vec1 = (*p) - robot.m_center;
    //在机器人正方向上的投影
    float proj_y = abs(vec1 * robot.m_axis[1]);
    float proj_x = abs(vec1 * robot.m_axis[0]);

    if (proj_x <= robot.m_halflength && proj_y <= robot.m_halfwidth) {
        return true;
    }
    else
        return false;
}


//递归遍历八叉树所有节点，得到碰撞的结果集，不可以用于判断激光器扫描到的区域，因为这个相当于z轴特化版
void getCollisionOctreeNodeSphere(OctreeNode* node, const Cuboid& robot) {
    if (node == nullptr)
        return;

    if (abortTesting(node->m_center[2] - node->m_halfheight,robot.m_center.m_z+robot.m_halfheight,\
                     node->m_center[2] + node->m_halfheight,robot.m_center.m_z-robot.m_halfheight))  //z轴不合适，直接扔
        return;

    if (isCollisionSphere(*node,robot)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            collisionnode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getCollisionOctreeNodeSphere(node->m_children[i], robot);
        }
    }
    else
        return;
}

void getCollisionOctreeNodeAABB(OctreeNode* node, const Cuboid& robot){
    if (node == nullptr)
        return;

    if (abortTesting(node->m_center[2] - node->m_halfheight,robot.m_center.m_z+robot.m_halfheight,\
                     node->m_center[2] + node->m_halfheight,robot.m_center.m_z-robot.m_halfheight))  //z轴不合适，直接扔
        return;

    if (isCollisionAABB(*node,robot)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            collisionnode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getCollisionOctreeNodeAABB(node->m_children[i], robot);
        }
    }
    else
        return;
}

void getCollisionOctreeNodeOBBThread(OctreeNode* node, const Cuboid& robot,std::vector<OctreeNode*>& collisionnode){
    if (node == nullptr)
        return;

    if (abortTesting(node->m_center[2] - node->m_halfheight,robot.m_center.m_z+robot.m_halfheight,\
                     node->m_center[2] + node->m_halfheight,robot.m_center.m_z-robot.m_halfheight))  //z轴不合适，直接扔
        return;

    if (isCollisionOBB(*node,robot)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            collisionnode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getCollisionOctreeNodeOBBThread(node->m_children[i], robot, collisionnode);
        }
    }
    else
        return;
}

void getCollisionOctreeNodeOBB(OctreeNode* node, const Cuboid& robot){
    if (node == nullptr)
        return;

    if (abortTesting(node->m_center[2] - node->m_halfheight,robot.m_center.m_z+robot.m_halfheight,\
                     node->m_center[2] + node->m_halfheight,robot.m_center.m_z-robot.m_halfheight))  //z轴不合适，直接扔
        return;

    if (isCollisionOBB(*node,robot)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            collisionnode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getCollisionOctreeNodeOBB(node->m_children[i], robot);
        }
    }
    else
        return;
}

void getCollisionOctreeNodeOBB2D(OctreeNode* node, const Cuboid& robot){
    if (node == nullptr)
        return;

    if (abortTesting(node->m_center[2] - node->m_halfheight,robot.m_center.m_z+robot.m_halfheight,\
                     node->m_center[2] + node->m_halfheight,robot.m_center.m_z-robot.m_halfheight))  //z轴不合适，直接扔
        return;

    if (isCollisionOBB2D(*node,robot)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            collisionnode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getCollisionOctreeNodeOBB2D(node->m_children[i], robot);
        }
    }
    else
        return;
}
//扫描三维包围球获得碰撞
bool isScanAABB(const Cuboid& octree, const Cuboid& laser){
    return (sqrt(((octree.m_center.m_x-laser.m_center.m_x)*(octree.m_center.m_x - laser.m_center.m_x)\
        + (octree.m_center.m_y - laser.m_center.m_y) * (octree.m_center.m_y - laser.m_center.m_y)\
        + (octree.m_center.m_z - laser.m_center.m_z) * (octree.m_center.m_z - laser.m_center.m_z)))
        <= (octree.m_R3 + laser.m_R3));
}

//获得包围球范围内的相交节点
void getScanOctreeNodeSphere(OctreeNode* node, const Cuboid& laser){
    if (node == nullptr)
        return;
    if (isScanAABB(*node,laser)) {
        if (node->m_children[0] == nullptr)  //自己就是叶子节点，直接写入结果集
            scannode.push_back(node);
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getScanOctreeNodeSphere(node->m_children[i], laser);
        }
    }
    else
        return;
}
//点是否在激光器的扫描范围内
bool isScanPoint(Point* p,const Cuboid& laser){
    return (sqrt(((p->m_x-laser.m_center.m_x)*(p->m_x - laser.m_center.m_x)\
        + (p->m_y - laser.m_center.m_y) * (p->m_y - laser.m_center.m_y)\
        + (p->m_z - laser.m_center.m_z) * (p->m_z - laser.m_center.m_z)))
        <= (laser.m_R3));
}
//获得包围球范围内的点
double getMinDistance(const std::vector<OctreeNode*>& scannode, const Cuboid& laser,const std::vector<Cuboid>& robotparts){
    //所有碰撞的节点
    double minsqDistance = 100000;
    for (const auto& num : scannode) {
        //该节点上的所有点
        Point* realhead = num->m_scenepointslist;
        while (realhead != nullptr) {
            if (isScanPoint(realhead, laser)) {
                for(const auto& robotpart : robotparts){
                    double distance = SqDistPointOBB(*realhead,robotpart);
                    minsqDistance = minsqDistance >  distance ? distance : minsqDistance;
                }
            }
            realhead = realhead->m_next;
        }
        realhead = num->m_obstaclepointslist;
        while (realhead != nullptr) {
            if (isScanPoint(realhead, laser)) {
                for(const auto& robotpart : robotparts){
                    double distance = SqDistPointOBB(*realhead,robotpart);
                    minsqDistance = minsqDistance >  distance ? distance : minsqDistance;
                }
            }
            realhead = realhead->m_next;
        }
    }
    if(minsqDistance == 100000)
        minsqDistance = LASERSCAN_R*LASERSCAN_R;

    return minsqDistance;
}

//扫描,获得被扫描到的点，如果可以直接得到点所在的八叉树节点就好了，现在我们可以直接获取到了
void laserScanning(OctreeNode* octree,const Cuboid& laser){
    scanpoint.clear();
    //scannode.clear();
    CELLTimestamp time;
    time.update();
    float timer = 0.0;
    for(float yawangle = 0; yawangle < 180; yawangle += 5){
        for(float pitangle = 0; pitangle < 360; pitangle += 5){
            Point startpoint(laser.m_center);
            Vector direction(cos(pitangle/180*PI)*cos(yawangle/180*PI),\
                             cos(pitangle/180*PI)*sin(yawangle/180*PI),\
                             sin(pitangle/180*PI));
            Ray ray(startpoint,direction);
            //光线发生相交
            std::tuple<Point*,OctreeNode*> des = getRayIntersectPoint(octree,ray);
            if(std::get<0>(des) != nullptr){
                scanpoint.insert(des);
                //scannode.push_back(std::get<1>(des));
            }
        }
    }


    //删除被扫描到的八叉树节点中的障碍物点，然后重新填充
    //deleteObject(scannode,laser);
//    for(const auto& pointandnode : scanpoint){
//        //如果这个点是障碍物点，将其插入到障碍物链表中
//        if(deduplication(std::get<0>(pointandnode),std::get<1>(pointandnode))){
//            //删除操作应该在这个里面做
//            OctreeNode::InsertObjectToObstacle(std::get<1>(pointandnode),std::get<0>(pointandnode));
//        }
//        //否则，该点是场景点，不做处理
//    }
    timer += time.getElapsedTimeInMilliSec();
    std::cout<<"scan once consumes time = "<<timer<<std::endl;
}
void laserScanningByNeighbor(Octree* octree,const Cuboid& laser,std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal>& result_set,const float& start_angle,const float& end_angle){
    result_set.clear();
    Point startpoint(laser.m_center);
    Vector direction(0,0,0);
    Ray ray(startpoint,direction);
    double laser_length = 4;
    double angle_resolution = 0.03;
    getVirtureSurroundPoint(octree,ray,laser_length,angle_resolution,start_angle,end_angle,result_set);

}
//重新填充被扫描到的八叉树节点(全部都是子节点，很好的性质),依我看，主要有两个步骤，一是删除，二是插入，此处，为删除
//删除只应该删除障碍物节点
void deleteObject(std::vector<OctreeNode*>& scannode,const Cuboid& laser){
    //删除，点到球心的距离小于半径则删除
    for(const auto& num : scannode){
        Point* pre = new Point();
        Point* virtualhead = pre;
        pre->m_next = num->m_obstaclepointslist;
        Point* cur = num->m_obstaclepointslist;
        while(cur != nullptr){
            //如果该点被扫描到,删除
            if(isScanPoint(cur,laser)){
                pre->m_next = cur->m_next;
                //delete cur;
                cur->m_next = nullptr;
                cur = pre->m_next;
            }
            else{
                pre = pre->m_next;
                cur = cur->m_next;
            }
        }
        num->m_obstaclepointslist = virtualhead->m_next;
        delete virtualhead;
    }
    //从UI中删除这个点
}
//采用暴力去重   参数：点  以及  该点所在的八叉树节点
bool deduplication(Point* point,OctreeNode* node){
    if(node == nullptr)
        return false;
    Point* curpoint = node->m_scenepointslist;
    while(curpoint != nullptr){
        if((*curpoint) == (*point))
            return false;
        curpoint = curpoint->m_next;
    }
    curpoint = node->m_obstaclepointslist;
    while(curpoint != nullptr){
        if((*curpoint) == (*point))
            return false;
        curpoint = curpoint->m_next;
    }
    return true;
}
//遍历结果集，得到碰撞发生的点
bool getCollisionPoint(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot) {
    //所有碰撞的节点
    for (const auto& num : collisionnode) {
        //该节点上的所有点
        Point* realhead = num->m_scenepointslist;
        while (realhead != nullptr) {
            if (isCollisionProject(realhead, robot)) {
                results.insert(realhead);
                return true;
            }
            else
                realhead = realhead->m_next;
        }
    }
    return false;
}

bool getCollisionPointThread(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot,std::unordered_set<Point*>& results) {
    //所有碰撞的节点
    for (const auto& num : collisionnode) {
        //该节点上的所有点
        Point* curpoint = num->m_scenepointslist;
        while (curpoint != nullptr) {
            if (isCollisionProject(curpoint, robot)) {
                results.insert(curpoint);
                return true;
            }
            else
                curpoint = curpoint->m_next;
        }
    }
    return false;
}

//光线和球体包围盒的相交测试 参数:球心，球半径，光线，距离----将点扩大到球，计算球和光线的相交测试
//基于点到直线的距离和韦达定理,如果相交，函数将返回光源到相交点的距离，如果不相交，返回-1，如果光源在包围球中，返回0
float getRayIntersectDistance(const Sphere& sphere, const Ray& light){
    Point tmp = light.m_startpoint - sphere.m_ballcenter;
    Vector m(tmp);
    float distance = -1;
    float b = m*light.m_direction;
    float c = m*m - sphere.m_radius*sphere.m_radius;
    if (c > 0.0f && b > 0.0f) return -1.f;
    float discr = b*b-c;
    // A negative discriminant corresponds to ray missing sphere
    if (discr < 0.0f) return -1.f;
    // Ray now found to intersect sphere, compute smallest t value of intersection
    distance = -b - std::sqrt(discr);
    // If t is negative, ray started inside sphere so clamp t to zero
    if (distance < 0.0f) distance = 0.0f;
    return distance;
}
//光线和AABB包围盒的相交测试 参数:光线，包围盒，距离----计算八叉树节点和光线的相交测试
//tmin将记录发生相交时的距离，这可能有助于我们筛选出最先和光线发生碰撞的节点
//基于分离轴理论
bool isRayIntersectAABB(const Ray& light, const Cuboid& AABB){

    //static const float EPSILON = 1e-6f;
    float tmin = 0.0f;          // set to -FLT_MAX to get first hit on line
    float tmax = FLT_MAX;     // set to max distance ray can travel (for segment)
    // For all three slabs
    for (int i = 0; i < 3; i++) {
        if (abs(light.m_direction[i]) < EPSILON) {
            // Ray is parallel to slab. No hit if origin not within slab
            if (light.m_startpoint[i] < AABB.m_center[i]-AABB[i] || light.m_startpoint[i] >  AABB.m_center[i]+AABB[i]) return false;
        } else {
            // Compute intersection t value of ray with near and far plane of slab
            float ood = 1.0f / light.m_direction[i];
            float t1 = (AABB.m_center[i]-AABB[i] - light.m_startpoint[i]) * ood;
            float t2 = (AABB.m_center[i]+AABB[i] - light.m_startpoint[i]) * ood;
            // Make t1 be intersection with near plane, t2 with far plane
            if (t1 > t2) std::swap(t1, t2);
            // Compute the intersection of slab intersections intervals
            tmin = std::max(tmin, t1);
            tmax = std::min(tmax, t2);
            // Exit with no collision as soon as slab intersection becomes empty
            if (tmin > tmax) return false;
        }
    }
    return true;
}
//获得所有和ray相交的八叉树节点
void getIntersectOctreeNode(OctreeNode* node,const Ray& light){
    if (node == nullptr)
        return;

    if (isRayIntersectAABB(light,*node)) {
        if (node->m_children[0] == nullptr){  //自己就是叶子节点，直接写入结果集
            //优化，如果距离太远，不予考虑
            Vector distance(light.m_startpoint-node->m_center);
            float  distance2 = pow(distance.m_x,2) + pow(distance.m_y,2) + pow(distance.m_z,2);
            if(distance2 < 25.0)
            rayintersectnode.push_back(node);
        }
        else {                               //不是叶子节点则递归调用
            for (int i = 0; i < 8; ++i)
                getIntersectOctreeNode(node->m_children[i], light);
        }
    }
    else
        return;
}
//从相交的所有八叉树节点中获得距离光源最近且和ray相交的点以及该点所在的八叉树节点
std::tuple<Point*,OctreeNode*> getIntersectPoint(const std::vector<OctreeNode*>& intersectnodes,const Ray& light){
    Point* intersectpoint = nullptr;
    OctreeNode* intersectnode = nullptr;
    float  mindistance = 100000;
    //所有碰撞的节点
    for (auto& node : intersectnodes) {
        //该节点上的所有点
        Point* realhead = node->m_scenepointslist;
        while (node->m_scenepointslist != nullptr) {
            float distance = getRayIntersectDistance(Sphere(*(node->m_scenepointslist),0.2f), light);
            if (distance >= 0 && distance < mindistance) {
                mindistance = distance;
                intersectpoint = node->m_scenepointslist;
                intersectnode  = node;
            }
            node->m_scenepointslist = node->m_scenepointslist->m_next;
        }
        node->m_scenepointslist = realhead;

        realhead = node->m_obstaclepointslist;
        while (node->m_obstaclepointslist != nullptr) {
            float distance = getRayIntersectDistance(Sphere(*(node->m_obstaclepointslist),0.2f), light);
            if (distance >= 0 && distance < mindistance) {
                mindistance = distance;
                intersectpoint = node->m_obstaclepointslist;
                intersectnode  = node;
            }
            node->m_obstaclepointslist = node->m_obstaclepointslist->m_next;
        }
        node->m_obstaclepointslist = realhead;

        auto point = node->m_noattributepointslist.begin();
        while(point !=  node->m_noattributepointslist.end()){
            float distance = getRayIntersectDistance(Sphere(*(*(point)),0.2f), light);
            if (distance >= 0 && distance < mindistance) {
                mindistance = distance;
                intersectpoint = *point;
                intersectnode  = node;
            }
                ++point;
        }

    }
    return {intersectpoint,intersectnode};
}

//从某一个节点中获取距离射线源最近的点,如果采用多线程，修改链表头指针的行为是绝对的错误！
//从此处可知，遍历链表的合理方法，应该是设计一个临时指针走一遍而不是直接拿着头指针走一遍
std::tuple<Point*,OctreeNode*> getIntersectPoint(OctreeNode* intersectnode,const Ray& light){
    Point* intersectpoint = nullptr;
    float  mindistance = 100000;
    Point* curpoint = intersectnode->m_scenepointslist;
    while (curpoint != nullptr) {
        float distance = getRayIntersectDistance(Sphere(*(curpoint),0.2f), light);
        if (distance >= 0 && distance < mindistance) {
            mindistance = distance;
            intersectpoint = curpoint;
        }
        curpoint = curpoint->m_next;
    }

    curpoint = intersectnode->m_obstaclepointslist;
    while (curpoint != nullptr) {
        float distance = getRayIntersectDistance(Sphere(*(curpoint),0.2f), light);
        if (distance >= 0 && distance < mindistance) {
            mindistance = distance;
            intersectpoint = curpoint;
        }
        curpoint = curpoint->m_next;
    }

//    auto point = intersectnode->m_noattributepointslist.begin();
//    while(point !=  intersectnode->m_noattributepointslist.end()){
//        float distance = getRayIntersectDistance(Sphere(*(*(point)),0.2f), light);
//        if (distance >= 0 && distance < mindistance) {
//            mindistance = distance;
//            intersectpoint = *point;
//        }
//            ++point;
//    }
    return {intersectpoint,intersectnode};
}
void getTrigonometricFunction(const double& angle_resolution){
    for(double angle = 0.0; angle < 2.0*PI; angle += angle_resolution){
        record_theta_tan.push_back(tan(angle));
        record_theta_cos.push_back(cos(angle));
        record_theta_sin.push_back(sin(angle));
    }
}
//
//输入参数octree------------参与检测的八叉树
//输入参数ray---------------参与检测的光线
//输入参数laser_length------光线的最大探测距离
//输入参数angle_resolution--角度分辨率
//有一些区域没有相交?
void getVirtureSurroundPoint(Octree* octree,Ray& ray,const double& laser_length,const double& angle_resolution,const float& start_angle,const float& end_angle,
                             std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal>& result_sets){
    //waynode.clear();
    double max_length = laser_length*laser_length;
    //起点W
    double org_x1 = ray.m_startpoint.m_x;
    double org_y1 = ray.m_startpoint.m_y;
    double org_z1 = ray.m_startpoint.m_z;
    int theta_i = 0;
    int alpha_i = 0;
//    CELLTimestamp stime;
//    double STIME = 0.F;
//    CELLTimestamp jtime;
//    double JTIME = 0.F;
//    CELLTimestamp ntime;
//    double NTIME = 0.F;
//    double theta_tan = record_theta_tan.at(0);
//    double theta_cos = record_theta_cos.at(0);
//    double theta_sin = record_theta_sin.at(0);
//    double alpha_tan = record_theta_tan.at(0);
//    double alpha_cos = record_theta_cos.at(0);
//    double alpha_sin = record_theta_sin.at(0);

    for(double alpha = 0.0 ;alpha < PI*2; alpha += angle_resolution){
        for(double theta = start_angle; theta < end_angle; theta += angle_resolution){

            //相关参数
//            if(theta_i < record_theta_tan.size() && alpha < record_theta_tan.size()){
//                theta_tan = record_theta_tan.at(theta_i);
//                theta_cos = record_theta_cos.at(theta_i);
//                theta_sin = record_theta_sin.at(theta_i);
//                alpha_tan = record_theta_tan.at(alpha_i);
//                alpha_cos = record_theta_cos.at(alpha_i);
//                alpha_sin = record_theta_sin.at(alpha_i);
//            }
//            else{
//                std::cerr<<"Program accesses illegal memory!\n";
//            }
            double theta_tan = tan(theta);
            double theta_cos = cos(theta);
            double theta_sin = sin(theta);
            double alpha_tan = tan(alpha);
            double alpha_cos = cos(alpha);
            double alpha_sin = sin(alpha);
            ++theta_i;
            Vector direction(alpha_cos*theta_cos,alpha_cos*theta_sin,alpha_sin);
            ray.m_direction = direction;
            //终点
            double org_x2 = ray.m_startpoint.m_x + laser_length*direction.m_x;
            double org_y2 = ray.m_startpoint.m_y + laser_length*direction.m_y;
            double org_z2 = ray.m_startpoint.m_z + laser_length*direction.m_z;

            //步长之后的点
            double x1 = org_x1;
            double y1 = org_y1;
            double z1 = org_z1;

            double length = 0;
            int count_num = 0;

            OctreeNode* lastnode = nullptr;

            Point curpoint(x1,y1,z1);

            //stime.m_axispdate();
            uint64_t MortanCode = octree->SearchNodeByPositionFromRoot(&(ray.m_startpoint));
            OctreeNode* node = octree->LookupNode(MortanCode);


            if(node == nullptr){
                std::cerr<<"The robot doesn't exist scene\n";
                break;
            }
            //STIME += stime.getElapsedTimeInMicroSec();
            //角度范围是 0~2PI? 陷入无限循环
            while(length < max_length&& count_num <= 40){
                direction_set direction = direction_set::FRONT;
                ++count_num;
                if(node == nullptr){
                    //Point* tmp = new Point(x1,y1,z1);
                    //result_sets.push_back(std::tuple<Point*,OctreeNode*>(tmp,nullptr));
                    //std::cout<<"result_sets.push_back(node == nullptr);"<<std::endl;
                    break;
                }
                if(node == lastnode){
                    std::cerr<<"\n\n\n";
                    std::cerr<<"Common node\n";
                    std::cerr<<"node.x = "<<node->m_center.m_x<<" "
                             <<"node.y = "<<node->m_center.m_y<<" "
                             <<"node.z = "<<node->m_center.m_z<<"\n";
                    std::cerr<<"node.front = "<<node->m_center.m_x + node->m_halflength<<" "
                             <<"node.right = "<<node->m_center.m_y + node->m_halfwidth<<" "
                             <<"node.top = "<<node->m_center.m_z + node->m_halfheight<<"\n";
                    std::cerr<<"x = "<<x1<<" "
                             <<"y = "<<y1<<" "
                             <<"z = "<<z1<<"\n";

                    std::cerr<<"count_num = "<<count_num<<"\n";
                    break;
                }
                //waynode.push_back(node);

                //光线和这个节点相交，对这个节点中的点和光线做相交测试,如果这个节点中真的存在点和光线相交，那么结束循环。
                if(node->m_scenepointslist != nullptr
                || node->m_obstaclepointslist != nullptr
                || !node->m_noattributepointslist.empty()){
                    //ntime.m_axispdate();
                    std::tuple<Point*,OctreeNode*> possible_result = getIntersectPoint(node,ray);
                    //NTIME += ntime.getElapsedTimeInMicroSec();
                    if(std::get<0>(possible_result) != nullptr)
                    {
                        result_sets.insert(possible_result);
//                        std::cout<<"x = "<<std::get<0>(possible_result)->m_x<<" "
//                                 <<"y = "<<std::get<0>(possible_result)->m_y<<" "
//                                 <<"z = "<<std::get<0>(possible_result)->m_z<<"\n";
                        break;
                    }
                }
                //jtime.m_axispdate();
                //Z方向分量为零，转化为二维问题
                if(EQ(org_z1,org_z2)){
                    if (EQ(org_y1, org_y2)) // horizontal line
                    {
                        if (GT(org_x1, org_x2)) {// x1 gets smaller
                            x1 = node->m_center.m_x-node->m_halflength-0.00001; // left edge
                            direction = direction_set::BACK;
                        }
                        else{
                            x1 = node->m_center.m_x+node->m_halflength + 0.00001;
                            direction = direction_set::FRONT;// right edge
                        }
#ifdef ROAD
                        std::cout<<"road_1.1\n";
#endif
                    }
                    else if (EQ(org_x1, org_x2)) // vertical line
                    {
                        if (GT(org_y1, org_y2)){ // y1 gets smaller
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001; // bottom edge
                            direction = direction_set::LEFT;
                        }
                        else{
                            y1 = node->m_center.m_y+node->m_halfwidth + 0.00001; // max edge
                            direction = direction_set::RIGHT;
                        }
#ifdef ROAD
                        std::cout<<"road_1.2\n";
#endif
                    }
                    else
                    {
                        if (GT(theta, 0.0)&&LT(theta,PI)) // up  上边缘
                        {
                            // ray could leave through the top edge
                            // solve x for known y
                            y1 = node->m_center.m_y+node->m_halfwidth + 0.00001; // top edge
                            x1 = (y1-org_y1)/theta_tan+org_x1;
                            // if the edge crossing was not in cell bounds
                            //std::cout<<"step_x1 = "<<x1<<" step_y1 = "<<y1<<"\n";
                            direction = direction_set::RIGHT;
#ifdef ROAD
                            std::cout<<"road_1.3.1\n";
#endif
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (GT(theta, PI / 2.0)){ // left side
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{ // right side
                                    x1 = node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1-org_x1)*theta_tan+org_y1;
#ifdef ROAD
                                std::cout<<"road_1.3.2\n";
#endif
                            }
                        }
                        else // down
                        {
                            // ray could leave through the bottom edge
                            // solve x for known y
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001; // bottom edge
                            x1 = (y1-org_y1)/theta_tan+org_x1;
                            direction = direction_set::LEFT;
#ifdef ROAD
                            std::cout<<"road_1.3.4\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (LT(theta,1.5*PI)){ // left side
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{
                                    x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1-org_x1)*theta_tan+org_y1;
#ifdef ROAD
                                std::cout<<"road_1.3.5\n";
#endif
                            }
                        }
                    }
                }
                //y方向分量为零，转化成二维问题
                else if(EQ(org_y1,org_y2)){
                    if (EQ(org_z1, org_z2)) // horizontal line
                    {
                        if (GT(org_x1, org_x2)){ // x1 gets smaller
                            x1 = node->m_center.m_x-node->m_halflength-0.00001; // left edge
                            direction = direction_set::BACK;
                        }
                        else{
                            x1 = node->m_center.m_x+node->m_halflength +0.00001; // right edge
                            direction = direction_set::FRONT;
                        }
#ifdef ROAD
                        std::cout<<"road_2.1\n";
#endif
                    }
                    else if (EQ(org_x1, org_x2)) // vertical line
                    {
                        if (GT(org_z1, org_z2)){ // y1 gets smaller
                            z1 = node->m_center.m_z-node->m_halfheight-0.00001; // bottom edge
                            direction = direction_set::BOTTOM;
                        }
                        else{
                            z1 = node->m_center.m_z+node->m_halfheight +0.00001; // max edge
                            direction = direction_set::BEYOND;
                        }
#ifdef ROAD
                        std::cout<<"road_2.2\n";
#endif
                    }
                    else
                    {
                        if (GT(alpha, 0.0)&&LT(alpha,PI)) // up  上边缘
                        {
                            // ray could leave through the top edge
                            // solve x for known y
                            z1 = node->m_center.m_z+node->m_halfheight + 0.00001; // top edge
                            x1 = (z1-org_z1)/alpha_tan+org_x1;
                            direction = direction_set::BEYOND;
#ifdef ROAD
                            std::cout<<"road_2.3\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (GT(alpha, PI / 2.0)) {// left side
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{ // right side
                                    x1 = node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                z1 = (x1-org_x1)*alpha_tan+org_z1;
#ifdef ROAD
                                std::cout<<"road_2.4\n";
#endif
                            }
                        }
                        else // down
                        {
                            // ray could leave through the bottom edge
                            // solve x for known y
                            z1 = node->m_center.m_z-node->m_halfheight-0.00001; // top edge
                            x1 = (z1-org_z1)/alpha_tan+org_x1;
                            direction = direction_set::BOTTOM;
#ifdef ROAD
                            std::cout<<"road_2.5\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (alpha < PI / 2.0 *3.0){ // left side
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{
                                    x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                z1 = (x1-org_x1)*alpha_tan+org_z1;
#ifdef ROAD
                                std::cout<<"road_2.6\n";
#endif
                            }
                        }
                    }
                }
                //x方向分量为零，转化成二维问题
                else if(EQ(org_x1,org_x2)){
                    if (EQ(org_z1, org_z2)) // horizontal line
                    {
                        if (GT(org_x1, org_x2)){ // x1 gets smaller
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001; // left edge
                            direction = direction_set::LEFT;
                        }
                        else{
                            y1 = node->m_center.m_y+node->m_halfwidth+0.00001; // right edge
                            direction = direction_set::RIGHT;
                        }
#ifdef ROAD
                        std::cout<<"road_3.1\n";
#endif
                    }
                    else if (EQ(org_y1, org_y2)) // vertical line
                    {
                        if (GT(org_z1, org_z2)){ // y1 gets smaller
                            z1 = node->m_center.m_z-node->m_halfheight-0.00001; // bottom edge
                            direction = direction_set::BOTTOM;
                        }
                        else{
                            z1 = node->m_center.m_z+node->m_halfheight+0.00001; // max edge
                            direction = direction_set::BEYOND;
                        }
#ifdef ROAD
                        std::cout<<"road_3.2\n";
#endif
                    }
                    else
                    {
                        if (GT(alpha, 0.0)&&LT(alpha,PI)) // up  上边缘
                        {
                            // ray could leave through the top edge
                            // solve x for known y
                            z1 = node->m_center.m_z+node->m_halfheight + 0.00001; // top edge
                            y1 = (z1-org_z1)/alpha_tan+org_y1;
                            direction = direction_set::BEYOND;
#ifdef ROAD
                            std::cout<<"road_3.3.1\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(y1, node->m_center.m_x-node->m_halfwidth) && LTE(y1, node->m_center.m_x+node->m_halfwidth)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (GT(alpha, PI / 2.0)){ // left side
                                    y1 = node->m_center.m_y-node->m_halfwidth-0.00001;
                                    direction = direction_set::LEFT;
                                }
                                else{ // right side
                                    y1 = node->m_center.m_y+node->m_halfwidth + 0.00001;
                                    direction = direction_set::RIGHT;
                                }
                                z1 = (y1-org_y1)*alpha_tan+org_z1;
#ifdef ROAD
                                std::cout<<"road_3.3.2\n";
#endif
                            }
                        }
                        else // down
                        {
                            // ray could leave through the bottom edge
                            // solve x for known y
                            z1 = node->m_center.m_z-node->m_halfheight - 0.0001; // top edge
                            y1 = (z1-org_z1)/alpha_tan+org_y1;
                            direction = direction_set::BOTTOM;
                            // if the edge crossing was not in cell bounds
#ifdef ROAD
                            std::cout<<"road_3.3.3\n";
#endif
                            if (!(GT(y1, node->m_center.m_x-node->m_halfwidth) && LTE(y1, node->m_center.m_x+node->m_halfwidth)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if (alpha < PI / 2.0 *3.0){ // left side
                                    y1 = node->m_center.m_y-node->m_halfwidth-0.00001;
                                    direction = direction_set::LEFT;
                                }
                                else{
                                    y1 =  node->m_center.m_y+node->m_halfwidth + 0.00001;
                                    direction = direction_set::RIGHT;
                                }
                                z1 = (y1-org_y1)*alpha_tan+org_z1;
#ifdef ROAD
                                std::cout<<"road_3.3.4\n";
#endif
                            }
                        }
                    }
                }
                //朝顶部打出 alpha = PI/0即z方向上的分量为零，已经讨论过
                else if(GT(alpha,0.0)&&LT(alpha,PI)){
                    //点在上面一层的中心格子
                    z1 = node->m_center.m_z + node->m_halfheight + 0.00001;
                    x1 = org_x1 + (z1 - org_z1)/alpha_tan*theta_cos;
                    y1 = org_y1 + (z1 - org_z1)/alpha_tan*theta_sin;
                    direction = direction_set::BEYOND;
#ifdef ROAD
                    std::cout<<"road_4.1\n";
#endif
                    //点在上面一层的右中/左中格子,则一定从left/right面穿过
                    if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                       &&(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){
                        if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                          ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))){
                            x1 = node->m_center.m_x-node->m_halflength-0.00001;
                            direction = direction_set::BACK;
                        }
                        else{
                            x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                            direction = direction_set::FRONT;
                        }
                        y1 = (x1 - org_x1)*theta_tan + org_y1;
                        z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                        std::cout<<"road_4.2\n";
#endif
                    }
                    //点在上面一层的上中/下中格子，则一定从front/back面穿过
                    else if((GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                    &&!(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){
                        if((GT(theta,0.0) && LT(theta,PI) && ((GT(alpha,0)&& LT(alpha,PI/2.0))||(GT(alpha,PI*1.5)&&LT(alpha,PI*2.0))))
                         ||(GT(theta,PI) && LT(theta,PI*2.0) && GT(alpha,PI/2.0) && LT(alpha,PI*1.5))){
                            y1 = node->m_center.m_y+node->m_halfwidth+0.00001;
                            direction = direction_set::RIGHT;
                        }
                        else{
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001;
                            direction = direction_set::LEFT;
                        }
                        x1 = (y1 - org_y1)/theta_tan + org_x1;
                        z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
#ifdef ROAD
                        std::cout<<"road_4.3\n";
#endif
                    }
                    //点在上面一层的四个角上的格子，仅仅依靠z1无法判断其从那个面打出，必须再固定一个轴
                    //不过此时可以判定其一定不是从beyond面打出，故问题也简化为二维，不过也要计算z1就是了
                    else if(!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                     &&!(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){                        //点在上左或者上右
                        if((GT(theta,0.0) && LT(theta,PI) && ((GT(alpha,0)&& LT(alpha,PI/2.0))||(GT(alpha,PI*1.5)&&LT(alpha,PI*2.0))))
                         ||(GT(theta,PI) && LT(theta,PI*2.0) && GT(alpha,PI/2.0) && LT(alpha,PI*1.5))) // up
                        {
                            //上边缘打出
                            y1 = node->m_center.m_y+node->m_halfwidth + 0.00001; // top edge
                            x1 = (y1 - org_y1)/theta_tan + org_x1;
                            z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
                            direction = direction_set::RIGHT;
#ifdef ROAD
                            std::cout<<"road_4.4.1\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                                  ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))){ // 左边缘打出
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{ // right side       // 右边缘打出
                                    x1 = node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1 - org_x1)*theta_tan + org_y1;
                                z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                                std::cout<<"road_4.4.2\n";
#endif
                            }
                        }
                        //点在下左或者下右
                        else // down
                        {
                            //下边缘打出
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001; // bottom edge
                            x1 = (y1 - org_y1)/theta_tan + org_x1;
                            z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
                            direction = direction_set::LEFT;
#ifdef ROAD
                            std::cout<<"road_4.5.1\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                                  ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))){ // 左边缘打出
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{                       // 右边缘打出
                                    x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1 - org_x1)*theta_tan + org_y1;
                                z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                                std::cout<<"road_4.5.2\n";
#endif
                            }
                        }
                    }
                }
                //朝底部打出，和顶部打出只有固定的z值不同
                else if(GT(alpha,PI)&&LT(alpha,2*PI)){
                    //点在下面一层的中心格子
                    z1 = node->m_center.m_z - node->m_halfheight-0.00001;
                    x1 = org_x1 + (z1 - org_z1)/alpha_tan*theta_cos;
                    y1 = org_y1 + (z1 - org_z1)/alpha_tan*theta_sin;
                    direction = direction_set::BOTTOM;
#ifdef ROAD
                    std::cout<<"road_5.1\n";
#endif
                    //点在上面一层的右中/左中格子,则一定从left/right面穿过
                    if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                       &&(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){
                        if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                          ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))) {//仅有theta无法确定从哪个面穿过，因为这是一个弧形
                            x1 = node->m_center.m_x-node->m_halflength-0.00001;
                            direction = direction_set::BACK;
                        }
                        else{
                            x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                            direction = direction_set::FRONT;
                        }
                        y1 = (x1 - org_x1)*theta_tan + org_y1;
                        z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                        std::cout<<"road_5.2\n";
#endif
                    }
                    //点在上面一层的上中/下中格子，则一定从front/back面穿过
                    else if((GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                    &&!(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){
                        if((GT(theta,0.0) && LT(theta,PI) && ((GT(alpha,0)&& LT(alpha,PI/2.0))||(GT(alpha,PI*1.5)&&LT(alpha,PI*2.0))))
                         ||(GT(theta,PI) && LT(theta,PI*2.0) && GT(alpha,PI/2.0) && LT(alpha,PI*1.5))){
                            y1 = node->m_center.m_y+node->m_halfwidth+0.00001;
                            direction = direction_set::RIGHT;
                            }
                        else{
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001;
                            direction = direction_set::LEFT;
                        }
                        x1 = (y1 - org_y1)/theta_tan + org_x1;
                        z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
#ifdef ROAD
                        std::cout<<"road_5.3\n";
#endif
                    }
                    //点在上面一层的四个角上的格子，仅仅依靠z1无法判断其从那个面打出，必须再固定一个轴
                    //不过由于此时可以判定其一定不是从beyond面打出，故问题也简化为二维，不过也要计算z1就是了
                    else if(!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength))
                     &&!(GT(y1, node->m_center.m_y-node->m_halfwidth) && LTE(y1, node->m_center.m_y+node->m_halfwidth))){
                        //点在上左或者上右
                        if((GT(theta,0.0) && LT(theta,PI) && ((GT(alpha,0)&& LT(alpha,PI/2.0))||(GT(alpha,PI*1.5)&&LT(alpha,PI*2.0))))
                         ||(GT(theta,PI) && LT(theta,PI*2.0) && GT(alpha,PI/2.0) && LT(alpha,PI*1.5))) // up
                        {
                            //上边缘打出
                            y1 = node->m_center.m_y+node->m_halfwidth + 0.00001; // top edge
                            x1 = (y1 - org_y1) / theta_tan + org_x1;
                            z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
                            direction = direction_set::RIGHT;
#ifdef ROAD
                            std::cout<<"road_5.4.1\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                // it must have left the cell on the left or right instead
                                // solve y for known x
                                if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                                  ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))){ // 左边缘打出
                                    x1 = node->m_center.m_x-node->m_halflength-0.00001;
                                    direction = direction_set::BACK;
                                }
                                else {// right side       // 右边缘打出
                                    x1 = node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1 - org_x1)*theta_tan + org_y1;
                                z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                                std::cout<<"road_5.4.2\n";
#endif
                            }
                        }
                        //点在下左或者下右
                        else // down
                        {
                            //下边缘打出
                            y1 = node->m_center.m_y-node->m_halfwidth-0.00001; // bottom edge
                            x1 = (y1 - org_y1) / theta_tan + org_x1;
                            z1 = (y1 - org_y1)/theta_sin*alpha_tan + org_z1;
                            direction = direction_set::LEFT;
#ifdef ROAD
                            std::cout<<"road_5.5.1\n";
#endif
                            // if the edge crossing was not in cell bounds
                            if (!(GT(x1, node->m_center.m_x-node->m_halflength) && LTE(x1, node->m_center.m_x+node->m_halflength)))
                            {
                                if((GT(theta,PI/2.0) && LT(theta,PI/2.0*3.0) && ((GT(alpha,0) && LT(alpha,PI/2.0))||(GT(alpha,PI*1.5) && LT(alpha,PI*2.0))))
                                  ||(((GT(theta,0)&&LT(theta,PI/2.0))||(GT(theta,PI*1.5)&&LT(theta,PI*2.0)))&& (GT(alpha,PI/2.0)&&LT(alpha,PI*1.5)))) {// 左边缘打出
                                    x1 = node->m_center.m_x-node->m_halflength - 0.00001;
                                    direction = direction_set::BACK;
                                }
                                else{                       // 右边缘打出
                                    x1 =  node->m_center.m_x+node->m_halflength + 0.00001;
                                    direction = direction_set::FRONT;
                                }
                                y1 = (x1 - org_x1)*theta_tan + org_y1;
                                z1 = (x1 - org_x1)/theta_cos*alpha_tan + org_z1;
#ifdef ROAD
                                std::cout<<"road_5.5.2\n";
#endif
                            }
                        }
                    }
                }

                length = (x1 - org_x1) *(x1 - org_x1) + (y1 - org_y1) * (y1 - org_y1) + (z1 - org_z1)*(z1 - org_z1);
                //JTIME += jtime.getElapsedTimeInMicroSec();
                //获得下一个点所在的节点
                //lastnode = node;

                Point curpoint(x1,y1,z1);

                //stime.m_axispdate();
                MortanCode = octree->SearchNodeByMortanCode(direction,MortanCode, &curpoint);
                node = octree->LookupNode(MortanCode);
                //STIME += stime.getElapsedTimeInMicroSec();
//                if(count_num >= 35){
//                    std::cout<<"There might is a error!\n";
//                    std::cout<<"tan(alpha) = "<<alpha_tan<<std::endl;
//                    std::cout<<"tan(theta) = "<<theta_tan<<std::endl;
//                    std::cout<<"z1-org_z1 = "<<(z1-org_z1)<<std::endl;
//                    std::cout<<"(y1-org_y1)/length = "<<(y1-org_y1)/length<<std::endl;
//                    std::cout<<"(x1-org_x1)/lenght = "<<(x1-org_x1)/length<<std::endl;
//                    std::cout<<"direction_set = "<<(int)direction<<std::endl;
//                    std::cout<<"Mortancode = "<<std::bitset<64>(MortanCode)<<std::endl;
//                    std::cout<<"lastMortae = "<<std::bitset<64>(MortanCode)<<std::endl;

//                }
            }
        }
        ++alpha_i;
        theta_i = 0;


    }

//    std::cout << "JTIME consumes time =  " << JTIME << std::endl;
//    std::cout << "STIME consumes time =  " << STIME << std::endl;
//    std::cout << "NTIME consumes time =  " << NTIME << std::endl;
}

//获得距离光源最近的相交点,和该点所在的八叉树节点
std::tuple<Point*,OctreeNode*> getRayIntersectPoint(OctreeNode* octree,Ray& light){
    //light.m_direction.normalized();
    rayintersectnode.clear();
    getIntersectOctreeNode(octree,light);
    return getIntersectPoint(rayintersectnode,light);
}
//返回p点距离OBB最近点的距离
double SqDistPointOBB(const Point& p,const Cuboid& OBB){
    Vector v = p - OBB.m_center;
    double sqDist = 0.0;

    double d = v * OBB.m_axis[0], excess = 0.0;
    if(d < -OBB.m_halflength)
        excess = d + OBB.m_halflength;
    else if (d > OBB.m_halflength)
        excess = d - OBB.m_halflength;
    sqDist += excess*excess;

    d = v * OBB.m_axis[1], excess = 0.0;
    if(d < -OBB.m_halfwidth)
        excess = d + OBB.m_halfwidth;
    else if (d > OBB.m_halfwidth)
        excess = d - OBB.m_halfwidth;
    sqDist += excess*excess;

    d = v * OBB.m_axis[2], excess = 0.0;
    if(d < -OBB.m_halfheight)
        excess = d + OBB.m_halfheight;
    else if (d > OBB.m_halfheight)
        excess = d - OBB.m_halfheight;
    sqDist += excess*excess;

    return sqDist;
}

Cuboid buildOBB(const Cuboid& A, const Cuboid& B){
    //获得两个主轴方向的最大最小投影
    float left,right, top, down;
    left = std::min(A.UL*A.m_axis[0],std::min(B.UL*A.m_axis[0],std::min(A.DL*A.m_axis[0],B.DL*A.m_axis[0])));
    right= std::max(A.UR*A.m_axis[0],std::max(B.UR*A.m_axis[0],std::max(A.DR*A.m_axis[0],B.DR*A.m_axis[0])));
    down = std::min(A.DL*A.m_axis[1],std::min(B.DL*A.m_axis[1],std::min(A.DR*A.m_axis[1],B.DR*A.m_axis[1])));
    top  = std::max(A.UR*A.m_axis[1],std::max(B.UR*A.m_axis[1],std::max(A.UL*A.m_axis[1],B.UL*A.m_axis[1])));

    //据此
    Cuboid OBB;
    OBB.m_axis[0] = A.m_axis[0];
    OBB.m_axis[1] = A.m_axis[1];
    OBB.m_halfheight = A.m_halfheight;
    OBB.m_halflength = (right - left)/2;
    OBB.m_halfwidth  = (top  -  down)/2;
    OBB.m_center.m_z = A.m_center.m_z;

    //将旋转坐标轴上的点重新映射到世界坐标轴上
    float partial_x = (right + left)/2;
    float partial_y = (top  +  down)/2;
    float global_x = partial_x*A.m_axis[0].m_x - partial_y*A.m_axis[0].m_y;
    float global_y = -partial_x*A.m_axis[1].m_x + partial_y*A.m_axis[1].m_y;
    OBB.m_center.m_x = global_x;
    OBB.m_center.m_y = global_y;

    return OBB;
}
Point operator+(const Point& lhs, const Point& rhs) {
    return Point{ lhs.m_x + rhs.m_x, lhs.m_y + rhs.m_y, lhs.m_z + rhs.m_z };
}
Point operator+(const Point& lhs, const Vector& rhs){
    return { lhs.m_x + rhs.m_x, lhs.m_y + rhs.m_y, lhs.m_z + rhs.m_z };
}
Point operator+(const Vector& lhs, const Point& rhs){
    return { lhs.m_x + rhs.m_x, lhs.m_y + rhs.m_y, lhs.m_z + rhs.m_z };
}
Point operator-(const Point& lhs, const Point& rhs) {
    return Point{ lhs.m_x - rhs.m_x, lhs.m_y - rhs.m_y, lhs.m_z - rhs.m_z };
}
#ifdef THREE_D
//点乘-1
float operator* (const Point& lhs, const Point& rhs) {
    return lhs.m_x * rhs.m_x + lhs.m_y * rhs.m_y + lhs.m_z * rhs.m_z ;
}
//点乘-2
Point operator* (const Point& lhs, const float& rhs) {
    return Point{ lhs.m_x * rhs, lhs.m_y * rhs, lhs.m_z * rhs };
}
#endif

#ifndef THREE_D
//点乘-1
float operator* (const Point& lhs, const Point& rhs) {
    return lhs.m_x * rhs.m_x + lhs.m_y * rhs.m_y ;
}
//点乘-2
Point operator* (const Point& lhs, const float& rhs) {
    return Point{ lhs.m_x * rhs, lhs.m_y * rhs, 0 };
}
#endif
//叉乘
#ifdef THREE_D
Point operator^ (const Point& lhs, const Point& rhs) {
    return{ lhs.m_y * rhs.m_z - lhs.m_z * rhs.m_y, lhs.m_z * rhs.m_x - lhs.m_x * rhs.m_z, lhs.m_x * rhs.m_y - lhs.m_y * rhs.m_x };
}
#endif

#ifndef THREE_D
Point operator^ (const Point& lhs, const Point& rhs){
    return {0,0,lhs.m_x*rhs.m_y-lhs.m_y*rhs.m_x};
}
#endif



//读取CSV文件
std::vector<std::vector<double>> read_gt_file(std::string filename){
    size_t count = 0;
    std::ifstream inFile(filename, std::ios::in);
    std::string lineStr;
    char delim=',';
    std::vector<std::vector<double>> matrix;
    if(inFile.fail()){
        std::cout <<"Read files fail....."<<std::endl;
        return matrix;
    }
    getline(inFile, lineStr);
    while (getline(inFile, lineStr)) {
        if(++count % 1 == 0)
        {
            std::stringstream ss(lineStr);
            std::string str;
            std::vector<double> lineArray;
            // cut apart by flag
            while (getline(ss, str, delim))
                lineArray.push_back(std::stod(str));
            matrix.push_back(lineArray);
        }
    }
    return matrix;
}
